Import yimWork

Const YIMPATHLOOPSTYLE_REPEAT:Int = 0
Const YIMPATHLOOPSTYLE_REVERSE:Int = 1

Class yimSpritePathConnector
	Field spr:yimSprite
	Field path:yimPath
	Field curPathBit:yimPathBit
	Field dir:Float = 1
	Field speed:Float = 0.0
	Field curPos:Float = 0.0
	
	Method New(_spr:yimSprite, _path:yimPath, _speed:Float, _dir:Int=1, _curPos:Float=0.0)
		spr = _spr
		path = _path
		dir = _dir
		speed = _speed
		curPos = _curPos
		curPathBit = path.pathBits.First()
		spr.SetPosition(curPathBit.startPoint.x, curPathBit.startPoint.y)
	End
	
	Method SetSpeed(spd:Float)
		speed = spd
	End
	
	Method SetCurPos(pos:Float)
		curPos = pos
	End
	
	Method SetDir(_dir:Int)
		dir = _dir
	End
	
	Method SetCurPathBit(pathBit:yimPathBit)
		curPathBit = pathBit
	End
	
	Method Advance()
	
		curPos += yimDelta(speed)*dir
		
		If dir = 1
			If curPos >= curPathBit.length
				If path.pathBits.Last() = curPathBit
					If path.loopStyle = YIMPATHLOOPSTYLE_REVERSE
						dir = -1	
					Else
						curPathBit = path.pathBits.First()
						spr.SetPosition(curPathBit.startPoint.x, curPathBit.startPoint.y)
						curPos = 0
					EndIf
				Else
					curPathBit = path.GetNextBit(curPathBit)
					curPos = 0
					spr.SetPosition(curPathBit.startPoint.x, curPathBit.startPoint.y)
				EndIf
			EndIf
		Else
			If curPos <= 0.0
				If path.pathBits.First() = curPathBit
					If path.loopStyle = YIMPATHLOOPSTYLE_REVERSE
						dir = 1
					Else
						curPathBit = path.pathBits.Last()
						spr.SetPosition(curPathBit.endPoint.x, curPathBit.endPoint.y)
						curPos = curPathBit.length
					EndIf
				Else
					curPathBit = path.GetPrevBit(curPathBit)
					curPos = curPathBit.length
					spr.SetPosition(curPathBit.endPoint.x, curPathBit.endPoint.y)
				EndIf
			EndIf
		EndIf
		
		curPathBit.AdvanceSprite(Self)
	
		'Debug("curPos: "+curPos)
		'Debug("dir: "+dir)
		'Debug("length: "+curPathBit.length)
		
	End
End

Class yimPath
	Field pathBits:List<yimPathBit> = New List<yimPathBit>()
	Field loopStyle:Int = YIMPATHLOOPSTYLE_REPEAT ' 0 to go back to start when end of path is reached, 1 to trace back
	
	Method AddPathBitArc(centerOffsetX:Float, centerOffsetY:Float, ang:Float)
		Local radius:Float = Sqrt(Pow(centerOffsetX, 2) + Pow(centerOffsetY, 2))
		
		
	End
	
	Method SetLoopStyle(style:Int)
		loopStyle = style
	End
	
	Method AddPathBitArc(centerX:Float, centerY:Float, radius:Float, startAng:Float, endAng:Float)
		Local tArc:= New yimPathBitArc()
		tArc.parent = Self
		tArc.centerPos.Set(centerX, centerY)
		tArc.radius = radius
		tArc.startAng = startAng
		tArc.endAng = endAng
		tArc.type = 1
		tArc.SetStartPoint(centerX + Sin(startAng)*radius,  centerY - Cos(startAng)*radius)
		tArc.SetEndPoint(centerX + Sin(endAng)*radius,  centerY - Cos(endAng)*radius)
		tArc.length = radius * ToRadians(endAng - startAng)
		pathBits.AddLast(tArc)
	End
	
	Method AddPathBitLine(x:Float, y:Float)
		Local tBit:= pathBits.Last()
		AddPathBitLine(tBit.endPoint.x, tBit.endPoint.y, tBit.endPoint.x+x, tBit.endPoint.y+y)
	End
	
	Method AddPathBitLine(x1:Float, y1:Float, x2:Float, y2:Float)
		Local tLine:= New yimPathBitLine()
		tLine.parent = Self
		tLine.SetStartPoint(x1, y1)
		tLine.SetEndPoint(x2, y2)
		tLine.length = Dist2D(x1, y1, x2, y2)
		pathBits.AddLast(tLine)
	End
	
	Method AddPathBitCurve(fa:Float[], isLoop:Bool=False)
		Local tCurve:= New yimPathBitCurve()

		If pathBits.Count() > 0
			Local tBit:= pathBits.Last()
			tCurve.AddPoint(tBit.endPoint.x, tBit.endPoint.y)
			tCurve.SetStartPoint(tBit.endPoint.x, tBit.endPoint.y)
		Else
			tCurve.SetStartPoint(fa[0], fa[1])
		EndIf
		
		For Local i:= 0 To (fa.Length-1) Step 2
			tCurve.AddPoint(fa[i], fa[i+1])
		Next
		
		tCurve.SetEndPoint(fa[fa.Length-2], fa[fa.Length-1])
		
		tCurve.parent = Self
		tCurve.length = tCurve.catmull.totalLength
		tCurve.catmull.isLoop = isLoop
		
		pathBits.AddLast(tCurve)
	End
	
	Method GetNextBit:yimPathBit(bit:yimPathBit)
		Local gotcha:Bool = False
		For Local tBit:= EachIn pathBits
			If gotcha
				Return tBit
			EndIf
			
			gotcha = (bit = tBit)
		Next
		
		Return bit
	End
	
	Method GetPrevBit:yimPathBit(bit:yimPathBit)
		Local lastBit:yimPathBit = bit
		For Local tBit:= EachIn pathBits
			If bit = tBit
				Return lastBit
			EndIf
			lastBit = tBit
		Next
		
		Return bit
	End
	
	Method Draw(eng:yimEngine)
		For Local tBit:= EachIn pathBits
			tBit.Draw(eng)
		Next
	End
End

Class yimPathBit
	Field startPoint:yimVec2D = New yimVec2D()
	Field endPoint:yimVec2D = New yimVec2D()
	Field parent:yimPath
	Field type:Int = 0
	Field length:Float = 0.0
	
	Method SetStartPoint(x:Float, y:Float)
		startPoint.Set(x, y)
	End
	
	Method SetEndPoint(x:Float, y:Float)
		endPoint.Set(x, y)
	End
	
	Method AdvanceSprite(conn:yimSpritePathConnector)

	End

	Method Draw(eng:yimEngine)
		
	End
End

Class yimPathBitArc Extends yimPathBit
	Field startAng:Float
	Field endAng:Float
	Field radius:Float
	Field centerPos:yimVec2D = New yimVec2D()
	
	Method AdvanceSprite(conn:yimSpritePathConnector)
		Local curAng:Float = startAng + ToDegrees(conn.curPos/radius)
		
		conn.spr.SetPosition(centerPos.x + Sin(curAng)*radius, centerPos.y - Cos(curAng)*radius)
	End
	
	Method Draw(eng:yimEngine)
		DrawArc(eng.MapToScreenX(centerPos.x), eng.MapToScreenY(centerPos.y), radius, startAng, endAng)
	End
End

Class yimPathBitCurve Extends yimPathBit
	Field catmull:yimCatmullRomCurve = New yimCatmullRomCurve()
	
	Method AddPoint(x:Float, y:Float)
		
		catmull.AddPoint(x, y)
	End
	
	Method AdvanceSprite(conn:yimSpritePathConnector)
		
		'Debug("OOOOP: "+conn.curPos)
		
		Local sprPos:yimVec2D = New yimVec2D()
		catmull.GetPoint(conn.spr.pos, conn.curPos)
		'Debug("X: "+sprPos.x+" Y: "+sprPos.y)

	End
	
	Method Draw(eng:yimEngine)
		Local t:Float = 0.0
		Local drawPoint:yimVec2D = New yimVec2D()
		
		'Local pA:yimVec2D[] = points.ToArray()
		Local numPoints:Int = catmull.points.Count()
		
		Local firstPoint:Int = 0
		Local lastPoint:Int = numPoints-1
		
		If catmull.isLoop
			firstPoint = numPoints-2
			lastPoint = 1
		EndIf
		
		While t <= catmull.totalLength
			
			catmull.GetPoint(drawPoint, t)
			DrawRect(eng.MapToScreenX(drawPoint.x), eng.MapToScreenY(drawPoint.y), 1.0, 1.0)
			
			t += 5.0
		End
	End
End

Class yimPathBitLine Extends yimPathBit
	
	Method AdvanceSprite(conn:yimSpritePathConnector)
		Local lineAng:Float = ATan2(endPoint.x - startPoint.x, endPoint.y - startPoint.y)
		conn.spr.SetPosition(startPoint.x + Sin(lineAng)*conn.curPos, startPoint.y + Cos(lineAng)*conn.curPos)

	End
	
	Method Draw(eng:yimEngine)
		DrawLine(eng.MapToScreenX(startPoint.x), eng.MapToScreenY(startPoint.y), eng.MapToScreenX(endPoint.x), eng.MapToScreenY(endPoint.y))
	End
End

Class yimCatmullRomCurve
	Field points:List<yimVec2D> = New List<yimVec2D>()
	Field pA:yimVec2D[]
	
	Field isLoop:Bool = False
	Field segLengths:Float[0]
	Field totalLength:Float
	
	Method AddPoint(x:Float, y:Float)
		points.AddLast(New yimVec2D(x, y))
		
		
		If points.Count() > 3
		
			Local t:Float = 0.0
			Local curPoint:yimVec2D = New yimVec2D()
				
			pA = points.ToArray()
			
			
			Local numPoints:Int = points.Count()
			
			Local lpA:yimVec2D[numPoints]
			
			Local li:Int = 0
			For Local tPoint:= EachIn points
				lpA[li] = New yimVec2D(tPoint.x, tPoint.y)
				li += 1
			Next
			
			Local segL:Float[numPoints-1]
			
			Local firstPoint:Int = 0
			Local lastPoint:Int = numPoints-1
			
			If isLoop
				firstPoint = numPoints-2
				lastPoint = 1
			EndIf
			
			While t <= 1.0
				
				CatmullRomPoint(curPoint, pA[firstPoint], pA[0], pA[1], pA[2], t)
				segL[0] += Dist2D(curPoint.x, curPoint.y, lpA[0].x, lpA[0].y)
				lpA[0].Copy(curPoint)
				
				For Local i:= 0 Until numPoints-3
					CatmullRomPoint(curPoint, pA[i], pA[i+1], pA[i+2], pA[i+3], t)
					segL[i+1] += Dist2D(curPoint.x, curPoint.y, lpA[i+1].x, lpA[i+1].y)
					lpA[i+1].Copy(curPoint)
				Next
				
				CatmullRomPoint(curPoint, pA[numPoints-3], pA[numPoints-2], pA[numPoints-1], pA[lastPoint], t)
				segL[numPoints-2] += Dist2D(curPoint.x, curPoint.y, lpA[numPoints-2].x, lpA[numPoints-2].y)
				lpA[numPoints-2].Copy(curPoint)
				
				t += 0.01
			Wend
			
			segLengths = segLengths.Resize(numPoints-1)
			
			totalLength = 0.0
			For Local i:= 0 Until numPoints-1
				segLengths[i] = segL[i]
				totalLength += segL[i]
			Next
		EndIf
	End
	
	Method GetSegByPos:Int(pos:Float)
		Local curSeg:Int = 0
		Local curLen:Float = 0.0
		
		For Local tSegL:= EachIn segLengths
			curLen += tSegL
			If curLen > pos
				Return curSeg
			EndIf
			
			curSeg += 1
		Next
		
		Return 0
	End
	
	Method GetSegLengthSum:Float(segStart:Int, segEnd:Int)
		Local curLen:Float = 0.0
		
		For Local i:= segStart Until segEnd
			curLen += segLengths[i]
		Next
		
		Return curLen
	End
	
	Method GetPoint(pOut:yimVec2D, t:Float)
		If t >= totalLength
			t = totalLength-0.001
		EndIf
		
		Local segs:Int = points.Count()-1
		Local seg:Int = GetSegByPos(t)
		
		Local pt:Float = (t - GetSegLengthSum(0, seg)) / segLengths[seg]
		
		'LogMe("segs: "+segs+" seg: "+seg+" pt: "+pt)
		'Return
		
		'Local pA:yimVec2D[] = points.ToArray()
		Local numPoints:Int = points.Count()
		
		Local firstPoint:Int = 0
		Local lastPoint:Int = numPoints-1
		
		If isLoop
			firstPoint = numPoints-2
			lastPoint = 1
		EndIf
		
		Select seg
			Case 0
				CatmullRomPoint(pOut, pA[firstPoint], pA[0], pA[1], pA[2], pt)
			Case numPoints-2
				CatmullRomPoint(pOut, pA[numPoints-3], pA[numPoints-2], pA[numPoints-1], pA[lastPoint], pt)
			Default
				CatmullRomPoint(pOut, pA[seg-1], pA[seg], pA[seg+1], pA[seg+2], pt)
		End
		
	End
	
	Method Draw()
		Local t:Float = 0.0
		Local drawPoint:yimVec2D = New yimVec2D()
		
		'Local pA:yimVec2D[] = points.ToArray()
		Local numPoints:Int = points.Count()
		
		Local firstPoint:Int = 0
		Local lastPoint:Int = numPoints-1
		
		If isLoop
			firstPoint = numPoints-2
			lastPoint = 1
		EndIf
		
		While t <= totalLength
			
			GetPoint(drawPoint, t)
			DrawRect(drawPoint.x, drawPoint.y, 1.0, 1.0)
			
			t += 5.0
		End
		
	End
End